
#include <iostream>
#include <Eigen/Dense>
#include <math.h>

#include "ArmDimensions.h"
#include "KinematicModelTypes.h"

dirKinResult dirKin(double teta1, double teta2, double teta3, double teta4);

invKinResult invKin(Point p);

invKinResult invKin(Point p, bool recursive){
	
	double x = p.getValue(Point::X);
	double y = p.getValue(Point::Y);
	double z = p.getValue(Point::Z);


	invKinResult res;

	res.reachable = false;

	double d0 = SHOULDERZ;
	double d1 = SHOULDERX;
	double a0 = SHOULDERY;
	double a2 = FIRST_LINK_LENGTH;
	double a3 = SECOND_LINK_LENGTH + HAND_HEIGHT + HAND_CENTER_OFFSET;

	double w = sqrt( (x-d1)*(x-d1) + (y-a0)*(y-a0) + (z-d0)*(z-d0) );

	
	double ctetak = -(a3*a3 - (w*w) - (a2*a2)) / (2.0*w*a2);
	double stetak = sqrt(1 - (ctetak*ctetak));
	double tetak = atan2(stetak, ctetak);
	double tetakd = tetak*180.0/M_PI;


	double cteta3 = ((w*ctetak)-a2)/a3;
	double steta3 = sqrt(1 - (cteta3*cteta3));
	double teta3 = atan2(steta3, cteta3);
	double teta3d = teta3*180/M_PI;


	double steta2;
	double cteta2;
	if( w > (a2+a3) ){

		steta2 = x-d1;
		cteta2 = sqrt( (z-d0)*(z-d0) + y*y );

	}
	else{

		steta2 = (x-d1)/(a2+(a3*cteta3));
		cteta2 = sqrt( 1 - steta2*steta2);

	}

	double teta2 = atan2(steta2, cteta2);
	double teta2d = teta2*180.0/M_PI;

	
	double ctetaw = y;
	double stetaw = (z-d0);
	double tetaw = atan2(stetaw, ctetaw);
	double k = w*(1-cos(teta2))*(ctetaw/w);
	double steta0 = k/w;
	double cteta0 = sqrt(1-(steta0*steta0));


	double teta0 = atan2(steta0, cteta0);
	if( (z-d0)>=0 )
		teta0 = -teta0;

	double teta0s = teta0*180/M_PI;
	stetaw = z-d0;

	tetaw = atan2(stetaw, ctetaw) + M_PI/2;
	double tetawd = tetaw*180/M_PI;



	double teta1 = tetaw - tetak - teta0;
	double teta1d = teta1*180/M_PI;

	//Vincoli (per ora quelli del corpo umano)
	if(teta1d >= -30 && teta1d <= 180   &&   teta2d >= -3 && teta2d <= 180   &&   teta3d >= 0 && teta3d <= 130   && w <= a2+a3){
			
		res.teta1 = teta1d;	
		res.teta2 = teta2d;
		res.teta3 = teta3d;


		res.reachable = true;

	}
	else{


		if(recursive){
			
			p.setX(p.getValue(Point::X) + 0.002);
			if(res.reachable == false)
				res = invKin(p, false);
			
			
			p.setX(p.getValue(Point::X) - 0.004);
			if(res.reachable == false)
				res = invKin(p, false);
			
			
			p.setX(p.getValue(Point::X) + 0.002);
			p.setY(p.getValue(Point::Y) + 0.002);
			if(res.reachable == false)
				res = invKin(p, false);
			
			
			p.setY(p.getValue(Point::X) - 0.004);
			if(res.reachable == false)
				res = invKin(p, false);
			
			
			p.setY(p.getValue(Point::Y) + 0.002);
			p.setZ(p.getValue(Point::Z) + 0.002);
			if(res.reachable == false)
				res = invKin(p, false);
			
			
			p.setZ(p.getValue(Point::Z) - 0.004);
			if(res.reachable == false)
				res = invKin(p, false);

		}
	}
	
	
	if(res.reachable == false && recursive == true)
			std::cout << "\nINVKIN: Grasping point (" << x << ", " << y << ", " << z << ") unreachable!" << std::endl;

	return res;
}



invKinResult invKin(Point p){

	return invKin(p, true);
	
}
